//
// Created by zhanglei on 2023/12/18.
//
#include "common/io_utils.h"

#include <glog/logging.h>
#include <unordered_map>
#include <opencv2/opencv.hpp>

namespace sad {
FullCloudPtr TransFullCloud(CloudPtr pc_in);
SimpleCloudPtr TransSimpleCloud(CloudPtr pc_in);

static int resolution_ = 0;
static double min_theta_ = 0.0;
static int num_scan_ = 16;
static double scale_ = 100.0;

void TxtIO::Go() {
    if (!fin) {
        LOG(ERROR) << "未能找到文件";
        return;
    }

    while (!fin.eof()) {
        std::string line;
        std::getline(fin, line);
        if (line.empty()) {
            continue;
        }

        if (line[0] == '#') {
            // 以#开头的是注释
            continue;
        }

        // load data from line
        std::stringstream ss;
        ss << line;
        std::string data_type;
        ss >> data_type;

        if (data_type == "IMU" && imu_proc_) {
            double time, gx, gy, gz, ax, ay, az;
            ss >> time >> gx >> gy >> gz >> ax >> ay >> az;
            // imu_proc_(IMU(time, Vec3d(gx, gy, gz) * math::kDEG2RAD, Vec3d(ax, ay, az)));
            imu_proc_(IMU(time, Vec3d(gx, gy, gz), Vec3d(ax, ay, az)));
        } else if (data_type == "ODOM" && odom_proc_) {
            double time, wl, wr;
            ss >> time >> wl >> wr;
            odom_proc_(Odom(time, wl, wr));
        } else if (data_type == "GNSS" && gnss_proc_) {
            double time, lat, lon, alt, heading;
            bool heading_valid;
            ss >> time >> lat >> lon >> alt >> heading >> heading_valid;
            gnss_proc_(GNSS(time, 4, Vec3d(lat, lon, alt), heading, heading_valid));
        }
    }

    LOG(INFO) << "done.";
}

void KaistCsvIO::Go() {
    if (!fin) {
        LOG(ERROR) << "未能找到文件";
        return;
    }
    
    std::vector<std::string> line_data_vec;
    line_data_vec.reserve(3);
    std::string line_str, value_str;

    std::unordered_map<std::string, std::string> time_encoder_map;
    std::unordered_map<std::string, std::string> time_imu_map;
    std::unordered_map<std::string, std::string> time_gps_map; 
    std::unordered_map<std::string, std::string> time_vrs_gps_map;
    std::unordered_map<std::string, std::string> time_sick_middle_map;
    std::unordered_map<std::string, std::string> time_velodyne_left_map;

    read_input_data = new read_data(file_path_);
    if(!read_input_data->read_only_kaist_data(time_encoder_map, time_imu_map, time_gps_map, time_vrs_gps_map, time_sick_middle_map, time_velodyne_left_map)) {
        return;
    }
    static int first_frame = 0;
    double first_timestamp = read_input_data->get_origin_timestamp();
    while (std::getline(fin, line_str)) {
        //LOG(INFO) << "line_str : " << line_str;
        line_data_vec.clear();
        std::stringstream ss(line_str);
        while (std::getline(ss, value_str, ',')) { line_data_vec.push_back(value_str); }
        const double kToSecond = 1e-9;
        if(line_data_vec.empty()) {
            continue;
        }
        const std::string& time_str = line_data_vec[0];

        const double timestamp = std::stod(time_str) * kToSecond - first_timestamp;

        const std::string& sensor_type = line_data_vec[1];

        //LOG(INFO) << "sensor_type : " << sensor_type;

        if (sensor_type == "imu" && imu_proc_) {

            if (time_imu_map.find(time_str) == time_imu_map.end()) {
                std::cout << "[main]: Failed to find imu data at time: " << time_str << std::endl;
                continue;
            }
            const std::string& imu_str = time_imu_map.at(time_str);
            //LOG(INFO) << "imu_str : " << imu_str;
            if(imu_str.size() < 17) {
                continue;
            }   
            std::stringstream imu_ss(imu_str);
            line_data_vec.clear();
            while (std::getline(imu_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            if(line_data_vec.empty()) {
                continue;
            }
            double time = timestamp;
            //LOG(INFO) << "line_data_vec : " << line_data_vec.size();
            double gx = std::stod(line_data_vec[8]);
            double gy = std::stod(line_data_vec[9]);
            double gz = std::stod(line_data_vec[10]);
            double ax = std::stod(line_data_vec[11]);
            double ay = std::stod(line_data_vec[12]);
            double az = std::stod(line_data_vec[13]);            
            imu_proc_(IMU(time, Vec3d(gx, gy, gz) * math::kDEG2RAD, Vec3d(ax, ay, az)));
            imu_proc_(IMU(time, Vec3d(gx, gy, gz), Vec3d(ax, ay, az)));
        } else if (sensor_type == "encoder" && odom_proc_) {
            if (time_encoder_map.find(time_str) == time_encoder_map.end()) {
                std::cout << "[main]: Failed to find encoder data at time: " << time_str << std::endl;
                continue;
            }

            const std::string& encoder_str = time_encoder_map.at(time_str);
            std::stringstream enc_ss(encoder_str);
            line_data_vec.clear();
            while (std::getline(enc_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            if(line_data_vec.empty()) {
                continue;
            }
            const double left_enc_cnt = std::stod(line_data_vec[1]);
            const double right_enc_cnt = std::stod(line_data_vec[2]);
            
            static bool is_init = false;
            static double init_left = 0;
            static double init_right = 0;
            if(!is_init) {
                init_left = left_enc_cnt;
                init_right = right_enc_cnt;
                last_left = left_enc_cnt;
                last_right = right_enc_cnt;
                is_init = true;
            }
            //LOG(INFO) << "init_left : " << init_left;
            //LOG(INFO) << "init_right : " << init_right;
            //LOG(INFO) << "left_enc_cnt : " << left_enc_cnt;
            //LOG(INFO) << "right_enc_cnt : " << right_enc_cnt;

            double time = timestamp;
            double wl = left_enc_cnt - last_left;
            double wr = right_enc_cnt - last_right;

            //double wl = left_enc_cnt - init_left - last_left;
            //double wr = right_enc_cnt - init_right - last_right;
            last_left = left_enc_cnt;
            last_right = right_enc_cnt;
            //LOG(INFO) << "wl : " << wl;
            //LOG(INFO) << "wr : " << wr;

            odom_proc_(Odom(time, wl, wr));
        } else if (sensor_type == "gps" && gnss_proc_) {
            if (time_gps_map.find(time_str) == time_gps_map.end()) {
                std::cout << "[main]: Failed to find gps data at time: " << time_str << std::endl;
                continue;
            }

            const std::string& gps_str = time_gps_map.at(time_str);

            std::stringstream gps_ss(gps_str);
            line_data_vec.clear();
            while (std::getline(gps_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            if(line_data_vec.empty()) {
                continue;
            }
            double time = timestamp;
            const double lat = std::stod(line_data_vec[1]);
            const double lon = std::stod(line_data_vec[2]);
            const double alt = std::stod(line_data_vec[3]);

            //Eigen::Matrix3d cov;
            //for (size_t i = 0; i < 9; ++i) {
            //    cov.data()[i] = std::stod(line_data_vec[4+i]);
            //}

            double latitude_std = std::stod(line_data_vec[4]);
            double longitude_std = std::stod(line_data_vec[5]);
            double altitude_std = std::stod(line_data_vec[6]);

            double heading = 0.0;
            bool heading_valid = true;
            
            gnss_proc_(GNSS(time, 4, Vec3d(lat, lon, alt), heading, heading_valid, Vec3d(latitude_std, longitude_std, altitude_std)));
        
        } else if (sensor_type == "vrs" && gnss_proc_) {
            if (time_vrs_gps_map.find(time_str) == time_vrs_gps_map.end()) {
                std::cout << "[main]: Failed to find gps data at time: " << time_str << std::endl;
                continue;
            }

            const std::string& gps_str = time_vrs_gps_map.at(time_str);
            std::stringstream gps_ss(gps_str);
            line_data_vec.clear();
            while (std::getline(gps_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            if(line_data_vec.empty()) {
                continue;
            }
            double time = timestamp;

            const double lat = std::stod(line_data_vec[1]);
            const double lon = std::stod(line_data_vec[2]);
            const double x = std::stod(line_data_vec[3]);
            const double y = std::stod(line_data_vec[4]);            
            const double alt = std::stod(line_data_vec[5]);
            int fix_state = std::stoi(line_data_vec[6]);
            int num_satellite = std::stoi(line_data_vec[7]);
            //LOG(INFO) << "timestamp : " << timestamp;

            //Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();
            //for (size_t i = 0; i < 3; ++i) {
                //cov[i][i] = std::stod(line_data_vec[8+i]);
            //}

            double latitude_std = std::stod(line_data_vec[8]);
            double longitude_std = std::stod(line_data_vec[9]);
            double altitude_std = std::stod(line_data_vec[10]);
            
            //LOG(INFO) << "latitude_std : " << latitude_std;
            //LOG(INFO) << "longitude_std : " << longitude_std;
            //LOG(INFO) << "altitude_std : " << altitude_std;
   
            int heading_valid_flag = std::stoi(line_data_vec[12]);
            bool heading_valid = (heading_valid_flag == 1); 
            double heading = std::stod(line_data_vec[13]);
            
            //LOG(INFO) << "heading_valid : " << heading_valid;
            //LOG(INFO) << "heading : " << heading;
            //gnss_proc_(GNSS(time, 4, Vec3d(lat, lon, alt), heading, heading_valid, Vec3d(latitude_std, longitude_std, altitude_std)));
        } else if (sensor_type == "sick_middle" && scan2d_proc_) {
            if (time_sick_middle_map.find(time_str) == time_sick_middle_map.end()) {
                std::cout << "[main]: Failed to find sick_middle data at time: " << time_str << std::endl;
                continue;
            }

            const std::string& sick_middle_str = time_sick_middle_map.at(time_str);
            std::stringstream sick_middle_ss(sick_middle_str);
            line_data_vec.clear();
            while (std::getline(sick_middle_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            
            //LOG(INFO) << "sick_middle_str : " << sick_middle_str;
            //LOG(INFO) << "line_data_vec : " << line_data_vec[0];
            if(line_data_vec.empty()) {
                continue;
            }
            std::ifstream file;

            std::string frame_file = file_path_ + "sensor_data/SICK_middle/" + line_data_vec[0] + ".bin";
            //LOG(INFO) << "frame_file : " << frame_file;
            file.open(frame_file, std::ios::in | std::ios::binary);
            Scan2d scan;
            while (!file.eof()) {
                float range;
                float intensity;
                file.read(reinterpret_cast<char *>(&range), sizeof(float));
                file.read(reinterpret_cast<char *>(&intensity), sizeof(float));
                scan.ranges.push_back(range);
                scan.intensities.push_back((intensity));
            }
            double degree_to_rad = 3.1415926536 / 180.0;
#if 1            
            scan.angle_min = -5.0 * degree_to_rad;
            scan.angle_max = 185.0 * degree_to_rad;
            scan.angle_increment = 0.666 * degree_to_rad;
#else 
            scan.angle_min = -1.65806281567;
            scan.angle_max = 1.65806281567;
            scan.angle_increment = 0.0116355288774;
#endif            
            scan.time_increment = 0.0;
            scan.scan_time = std::stod(line_data_vec[0]) * 0.000000001 - first_timestamp; // ns to s
            scan.range_min = 0.0;
            scan.range_max = 81.0;

            //LOG(INFO) << "scan.scan_time : " << scan.scan_time;

            file.close();

            scan2d_proc_(scan);

            //LOG(INFO) << "timestamp : " << timestamp;
        } else if(sensor_type == "velodyne_left") {
            if (time_velodyne_left_map.find(time_str) == time_velodyne_left_map.end()) {
                std::cout << "[main]: Failed to find sick_middle data at time: " << time_str << std::endl;
                continue;
            }

            const std::string& velodyne_left_str = time_velodyne_left_map.at(time_str);
            std::stringstream velodyne_left_ss(velodyne_left_str);
            line_data_vec.clear();
            while (std::getline(velodyne_left_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            
            //LOG(INFO) << "line_data_vec : " << line_data_vec[0];
            const std::string velodyne_left_file = file_path_ + "sensor_data/VLP_left/" + line_data_vec[0] + ".bin"; 
                
            std::ifstream file;
            file.open(velodyne_left_file, std::ios::in | std::ios::binary);
            if(!file.is_open()) {
                continue;
            }

            CloudPtr cloud_ptr;
            cloud_ptr.reset(new pcl::PointCloud<pcl::PointXYZI>());
            SimpleCloudPtr simple_cloud_ptr;
            simple_cloud_ptr.reset(new pcl::PointCloud<SimplePointType>());

            while (!file.eof()) {
                float x, y, z, intensity;
                file.read(reinterpret_cast<char *>(&x), sizeof(float));
                file.read(reinterpret_cast<char *>(&y), sizeof(float));
                file.read(reinterpret_cast<char *>(&z), sizeof(float));
                file.read(reinterpret_cast<char *>(&intensity), sizeof(float));

                //std::cout << " lidar point : " << x << " , " << y << " , " << z << " , " << intensity << std::endl;
                if(used_simple_point && first_frame == 0) {
                    pcl::PointXYZI point_cloud;
                    point_cloud.x = x;
                    point_cloud.y = y;
                    point_cloud.z = z;
                    point_cloud.intensity = intensity;
                    cloud_ptr->points.push_back(point_cloud);
                } else if(used_simple_point) {
                    SimplePointType simple_point_cloud;
                    simple_point_cloud.timestamp = timestamp;
                    simple_point_cloud.x = x;
                    simple_point_cloud.y = y;
                    simple_point_cloud.z = z;
                    simple_point_cloud.intensity = intensity;
                    double dist = std::sqrt(x*x+y*y+z*z);
                    double theta = asin(z/dist) * scale_;      
                    int scan_id = (theta - min_theta_) / resolution_;
                    if(scan_id >= num_scan_) {
                        scan_id = num_scan_-1;
                    }
                    if(scan_id < 0) {
                        scan_id = 0;
                    }  

                    //double angle = atan2(z, std::sqrt(x * x + y * y)) / 3.14159265 * 180 + 15;
                    //int ring = round(angle / 2);
                    //LOG(INFO) << "scan_id ======= : " << scan_id;
                    //LOG(INFO) << "ring ======= : " << ring;

                    simple_point_cloud.ring = scan_id;
                    simple_cloud_ptr->points.push_back(simple_point_cloud);
                } else {
                    pcl::PointXYZI point_cloud;
                    point_cloud.x = x;
                    point_cloud.y = y;
                    point_cloud.z = z;
                    point_cloud.intensity = intensity;
                    cloud_ptr->points.push_back(point_cloud);    
                }
            }

            file.close();

            if(!used_simple_point) {
                cloud3d_proc_(cloud_ptr);
            } else {
                if(first_frame == 0) {
                    simple_cloud_ptr = TransSimpleCloud(cloud_ptr);
                    first_frame++;
                }
                simple_cloud_proc_(simple_cloud_ptr);                
            }

        }  else if (sensor_type == "stereo") {
               if(false) {
                  const std::string img0_file = file_path_ + "image/stereo_left/" + time_str + ".png"; 
                  const std::string img1_file = file_path_ + "image/stereo_right/" + time_str + ".png"; 
                  cv::Mat im_left = cv::imread(img0_file);
                  cv::Mat im_right = cv::imread(img1_file);
                  cv::imshow("image", im_left);
                  cv::waitKey(1);
               }
            }
    }

    LOG(INFO) << "done.";
}

FullCloudPtr TransFullCloud(CloudPtr pc_in) {
    FullCloudPtr full_clouds(new pcl::PointCloud<FullPointType>());
    double max_theta = -10000.0;
    double min_theta = 10000.0;
    double scale = 100.0;
    std::vector<double> all_theta;
    for (auto &p : pc_in->points) {
        double dist = std::sqrt(p.x*p.x+p.y*p.y+p.z*p.z);
        double theta = asin(p.z/dist) * scale;      
        all_theta.push_back(theta);
        if(theta > max_theta) {
            max_theta = theta;
        }

        if(theta < min_theta) {
            min_theta = theta;
        }   
    }

    int num_scan = 16;
    double resolution = (max_theta - min_theta) / 16.0 + 0.000001;

    resolution_ = resolution;
    min_theta_ = min_theta;
    scale_ = scale;
    num_scan_ = num_scan;
    //LOG(INFO) << " resolution ========= : " << resolution;
    //LOG(INFO) << " max_theta ========= : " << max_theta;
    //LOG(INFO) << " min_theta ========= : " << min_theta;

    for(int id = 0; id < all_theta.size(); id++) {
        auto pt = pc_in->points[id];
        double theta = all_theta[id];
        int scan_id = (theta - min_theta) / resolution;
        if(scan_id >= num_scan) {
            scan_id = num_scan-1;
        }
        if(scan_id < 0) {
            scan_id = 0;
        }        

        FullPointType full_cloud;
        full_cloud.x = pt.x;
        full_cloud.y = pt.y;
        full_cloud.z = pt.z;
        full_cloud.ring = scan_id;
        full_cloud.intensity = pt.intensity;
        full_clouds->points.push_back(full_cloud);
    }

    return full_clouds;
}


SimpleCloudPtr TransSimpleCloud(CloudPtr pc_in) {
    SimpleCloudPtr simple_clouds(new pcl::PointCloud<SimplePointType>());
    double max_theta = -10000.0;
    double min_theta = 10000.0;
    double scale = 100.0;
    std::vector<double> all_theta;
    for (auto &p : pc_in->points) {
        double dist = std::sqrt(p.x*p.x+p.y*p.y+p.z*p.z);
        double theta = asin(p.z/dist) * scale;      
        all_theta.push_back(theta);
        if(theta > max_theta) {
            max_theta = theta;
        }

        if(theta < min_theta) {
            min_theta = theta;
        }   
    }

    int num_scan = 16;
    double resolution = (max_theta - min_theta) / 16.0 + 0.000001;

    if(fabs(resolution - 3.27249) > 0.2) {
        resolution = 3.27249;
    }
    if(fabs(min_theta + 26.1799) > 0.2) {
        min_theta = 26.1799;
    } 

    resolution_ = resolution;
    min_theta_ = min_theta;
    scale_ = scale;
    num_scan_ = num_scan;

    //LOG(INFO) << " resolution ========= : " << resolution;
    //LOG(INFO) << " max_theta ========= : " << max_theta;
    //LOG(INFO) << " min_theta ========= : " << min_theta;

    for(int id = 0; id < all_theta.size(); id++) {
        auto pt = pc_in->points[id];
        double theta = all_theta[id];
        int scan_id = (theta - min_theta) / resolution;
        if(scan_id >= num_scan) {
            scan_id = num_scan-1;
        }
        if(scan_id < 0) {
            scan_id = 0;
        }        

        SimplePointType simple_cloud;
        simple_cloud.x = pt.x;
        simple_cloud.y = pt.y;
        simple_cloud.z = pt.z;
        simple_cloud.ring = scan_id;
        simple_cloud.intensity = pt.intensity;
        simple_clouds->points.push_back(simple_cloud);
    }

    return simple_clouds;
}

}  // namespace sad